// --------------------------------------------------------------------------------------------------------------------
// <copyright file="CHOMPparameters.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Manipulation
{
    /// <summary>
    /// Parameters for the optimization algorithm CHOMP - allows easy modification and access to the parameters for invoking a CHOMP planner
    /// </summary>
    public class CHOMPparameters
    {
        /// <summary>
        /// Initializes a new instance of the <see cref="CHOMPparameters" /> class
        /// All parameters are set to their default values and the user can also pass in desired values.
        /// </summary>
        /// <param name="numPoints"> Number of points on the trajectory from start to goal (including both) </param>
        /// <param name="smoothWeight"> Weight for the smoothness cost </param>
        /// <param name="obsWeight"> Weight for the obstacle cost </param>
        /// <param name="envCollisionTolerance"> Tolerance for collision with environment. This is used in conjunction with the delegate function that supplies obstacle costs and gradients </param>
        /// <param name="selfCollisionTolerance"> Self-collision tolerance distance. Links with distance less than this value incur self-collision cost </param>
        /// <param name="envCollisionFactor"> Scale factor for cost from external obstacles </param>
        /// <param name="selfCollisionFactor"> Scale factor for cost from self collision </param>
        /// <param name="useSelfCollisionInConvergenceTesting"> Flag that tells us if we want to use self collision check for CHOMP convergence testing </param>
        /// <param name="stepSize"> Step size for gradient descent </param>
        /// <param name="maxTime"> Maximum time to run the CHOMP optimization (in seconds). Does not include initial constructor time. </param>
        /// <param name="maxIterations"> Maximum number of iterations to run CHOMP. </param>
        /// <param name="minCostDifference"> Minimum allowable cost difference between two iterations of CHOMP. </param>
        /// <param name="exitOnNoCollision"> Flag that determines if we exit from CHOMP when we get a collision free trajectory. </param>
        public CHOMPparameters(
            int numPoints = 101,
            double smoothWeight = 1.0,
            double obsWeight = 1.0,
            double envCollisionTolerance = 0.05,
            double selfCollisionTolerance = 0.02,
            double envCollisionFactor = 200.0,
            double selfCollisionFactor = 20.0,
            bool useSelfCollisionInConvergenceTesting = true,
            double stepSize = 1.0 / 250,
            double maxTime = 10.0,
            int maxIterations = 100,
            double minCostDifference = 0.00005,
            bool exitOnNoCollision = true)
        {
            // Set number of points on the trajectory
            this.NumPoints = numPoints;

            // Set the smoothness and obstacle weight
            this.SmoothWeight = smoothWeight;
            this.ObsWeight = obsWeight;

            // Tolerance for collision with environment
            this.EnvCollisionTolerance = envCollisionTolerance;

            // Self collision tolerance
            this.SelfCollisionTolerance = selfCollisionTolerance;

            // Scale factor for the obstacle avoidance cost
            this.EnvCollisionFactor = envCollisionFactor;

            // Scale factor for self collision cost
            this.SelfCollisionFactor = selfCollisionFactor;

            // Flag that tells us if we want to use self collision check for CHOMP convergence testing
            this.UseSelfCollisionInConvergenceTesting = useSelfCollisionInConvergenceTesting;

            // Step size for gradient descent
            this.StepSize = stepSize;

            // Max time to run CHOMP iterations 
            this.MaxTime = maxTime;

            // Number of gradient descent iterations 
            this.MaxIterations = maxIterations;

            // Minimum cost difference between subsequent iterations of CHOMP
            this.MinCostDifference = minCostDifference;

            // Flag for determining if we exit when the trajectory is not in collision
            this.ExitOnNoCollision = exitOnNoCollision;
        }

        /// <summary>
        /// Gets or sets the number of points on the trajectory (including start and goal points)
        /// </summary>
        public int NumPoints { get; set; }

        /// <summary>
        /// Gets or sets the Weight for the smoothness cost
        /// </summary>
        public double SmoothWeight { get; set; }

        /// <summary>
        /// Gets or sets the Weight for the Obstacle cost
        /// </summary>
        public double ObsWeight { get; set; }

        /// <summary>
        /// Gets or sets the Tolerance for collision with obstacles in the environment. Spheres incur a self collision cost if distance between them and obstacles is less than this value
        /// </summary>
        public double EnvCollisionTolerance { get; set; }

        /// <summary>
        /// Gets or sets the Tolerance for self-collision. Spheres incur a self collision cost if distance between them is less than this value
        /// </summary>
        public double SelfCollisionTolerance { get; set; }

        /// <summary>
        /// Gets or sets the Scale factor for collision with obstacles in the environment. Cost and gradient w.r.t obstacles are scaled by this factor
        /// </summary>
        public double EnvCollisionFactor { get; set; }

        /// <summary>
        /// Gets or sets the Scale factor for self-collision. Cost and gradient w.r.t self-collision are scaled by this factor
        /// </summary>
        public double SelfCollisionFactor { get; set; }

        /// <summary>
        /// Gets or sets a value indicating whether we want to use self collision check in CHOMP convergence
        /// </summary>
        public bool UseSelfCollisionInConvergenceTesting { get; set; }

        /// <summary>
        /// Gets or sets the Step size for gradient descent
        /// </summary>
        public double StepSize { get; set; }

        /// <summary>
        /// Gets or sets the Max time to run CHOMP iterations  (in seconds)
        /// </summary>
        public double MaxTime { get; set; }

        /// <summary>
        /// Gets or sets the Number of gradient descent iterations 
        /// </summary>
        public int MaxIterations { get; set; }

        /// <summary>
        /// Gets or sets the Minimum cost difference between subsequent iterations of CHOMP
        /// </summary>
        public double MinCostDifference { get; set; }

        /// <summary>
        /// Gets or sets a value indicating whether CHOMP exits when we have reached a collision free trajectory. Default is "True"
        /// </summary>
        public bool ExitOnNoCollision { get; set; }
    }
}
